/****
 *
 *  运动传感器处理模块，目前支持 MPU6050, I2C总线，
 *  hbmd1 为 MPU6050传感器数据处理句柄，对外暴露一个接口
 *
 */
#include "bsp_motion_sensor.h"

#include <stdio.h>

#include "ax_mpu6050.h"
#include "bsp_buzzer.h"
#include "bsp_def.h"

// MPU6050灵敏度设置（根据你的配置调整）
#define ACCEL_SENSITIVITY 16384.0  // ±2g
#define GYRO_SENSITIVITY 131.0     // ±250°/s
#define CALIBRATION_SAMPLES 50     // 校准次数

BSP_MoveDetect_HandleTypeDef hbmd1 = {0};  /// 移动检测

static BSP_MoveDetect_HandleTypeDef* hbmd_ptr = &hbmd1;

static void MPU6050_Init(I2C_HandleTypeDef* hi2c);
static void MPU6050_Loop();
void BSP_MoveDetect_Init() {
    MPU6050_Init(BSP_MPU6050_I2C);  // 传入接口
}
// 方便型号扩展
void BSP_MoveDetect_Loop() {
    MPU6050_Loop();
}

static void MPU6050_Init(I2C_HandleTypeDef* hi2c) {
    // 初始化MPU6050
    AX_MPU6050_Init(hi2c);

    // 测试连接
    if (AX_MPU6050_TestConnection() == 0) {
        printf("MPU6050 Connected Successfully!\r\n");
    } else {
        printf("MPU6050 Connection Failed!\r\n");
        // while (1);
    }

    // 清零所有累积和偏移量
    for (int i = 0; i < 3; i++) {
        hbmd_ptr->acc_offset[i] = 0;
        hbmd_ptr->gyro_offset[i] = 0;
        hbmd_ptr->acc_sum[i] = 0;
        hbmd_ptr->gyro_sum[i] = 0;
    }
    hbmd_ptr->counter = 0;
    hbmd_ptr->calibration_done = 0;

    // 可选：设置量程和滤波器
    AX_MPU6050_SetAccRange(ACC_RANGE_2G);
    AX_MPU6050_SetGyroRange(GYRO_RANGE_250);
    AX_MPU6050_SetDLPF(DLPF_ACC44_GYRO42);
}

static void MPU6050_Loop() {
    if (hbmd_ptr == NULL) {
        return;
    }
    // MPU采样点数据
    int16_t acc_data[3];
    int16_t gyro_data[3];
    float temperature = 0;

    // 1.读取传感器数据
    AX_MPU6050_GetAccData(acc_data);
    AX_MPU6050_GetGyroData(gyro_data);
    temperature = AX_MPU6050_GetTempValue();

    // 2. 零偏校准逻辑
    if (hbmd_ptr->counter < CALIBRATION_SAMPLES) {
        // 校准阶段：累积采样数据
        for (int i = 0; i < 3; i++) {
            hbmd_ptr->acc_sum[i] += acc_data[i];
            hbmd_ptr->gyro_sum[i] += gyro_data[i];
        }

        // 最后一次校准采样，计算平均值
        if (hbmd_ptr->counter == CALIBRATION_SAMPLES - 1) {
            for (int i = 0; i < 3; i++) {
                hbmd_ptr->acc_offset[i] = hbmd_ptr->acc_sum[i] / CALIBRATION_SAMPLES;
                hbmd_ptr->gyro_offset[i] = hbmd_ptr->gyro_sum[i] / CALIBRATION_SAMPLES;
            }
            // 累加器清零作为后面探测使用
            for (int i = 0; i < 3; i++) {
                hbmd_ptr->acc_sum[i] = 0;
                hbmd_ptr->gyro_sum[i] = 0;
            }
            BuzzerBeep(1000, 1);
            printf("MPU6050 calibration complete. Samples: %d\r\n", CALIBRATION_SAMPLES);
        }
// 校准期间不输出有效数据
#if DEBUG_MOVIE_DETECT
        printf("Calibrating... %d/%d\r\n", hbmd_ptr->counter + 1, CALIBRATION_SAMPLES);
#endif
        hbmd_ptr->counter++;
        return;
    }

    // 2. 数据校准和转换
    for (int i = 0; i < 3; i++) {
        hbmd_ptr->mpu_data.acc[i] = (acc_data[i] - hbmd_ptr->acc_offset[i]) / ACCEL_SENSITIVITY;
        hbmd_ptr->mpu_data.gyro[i] = (gyro_data[i] - hbmd_ptr->gyro_offset[i]) / GYRO_SENSITIVITY;
        hbmd_ptr->mpu_sum.acc[i] = hbmd_ptr->mpu_data.acc[i];
        hbmd_ptr->mpu_sum.gyro[i] += hbmd_ptr->mpu_data.gyro[i];
    }

    hbmd_ptr->mpu_data.temperature = temperature;

#if DEBUG_MOVIE_DETECT > 0
    // 打印数据
    printf("MPU6050 Loop\r\n");
    printf("Acc: X=%.2f, Y=%.2f, Z=%.2f\r\n", hbmd_ptr->mpu_data.acc[0], hbmd_ptr->mpu_data.acc[1], hbmd_ptr->mpu_data.acc[2]);
    printf("Gyro: X=%.2f, Y=%.2f, Z=%.2f\r\n", hbmd_ptr->mpu_data.gyro[0], hbmd_ptr->mpu_data.gyro[1], hbmd_ptr->mpu_data.gyro[2]);
    printf("Temp: %.2f C\r\n\r\n", temperature);
    printf("Acc Sum: X=%.2lf, Y=%.2lf, Z=%.2lf\r\n", hbmd_ptr->mpu_sum.acc[0], hbmd_ptr->mpu_sum.acc[1], hbmd_ptr->mpu_sum.acc[2]);
    printf("Gyro Sum: X=%.2lf, Y=%.2lf, Z=%.2lf\r\n", hbmd_ptr->mpu_sum.gyro[0], hbmd_ptr->mpu_sum.gyro[1], hbmd_ptr->mpu_sum.gyro[2]);
    printf("Counter: %.d", hbmd_ptr->counter); 
    hbmd_ptr->counter++;  // 计数
#endif
}